#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/u_int8_multi_array.hpp"
#include <opencv2/opencv.hpp>

#include "final_exam/param.h"

class ScreenNode : public rclcpp::Node {
public:
  ScreenNode(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "Screen Node '%s' has been started.",
                name.c_str());
    // 订阅标注后图像话题（使用 UInt8MultiArray 接收二进制数据）
    annotated_image_subscription_ =
        this->create_subscription<std_msgs::msg::UInt8MultiArray>(
            "annotated_image_topic", 10,
            [&](const std_msgs::msg::UInt8MultiArray::SharedPtr msg) {
              annotatedImageCallback(msg);
            });
  }

private:
  // 标注后图像话题订阅者
  rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr
      annotated_image_subscription_;

  // 标注后图像话题回调函数
  void
  annotatedImageCallback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "Received annotated image, size: %zu bytes",
                msg->data.size());
    // 对图像进行解码（直接使用 msg->data）
    cv::Mat img = cv::imdecode(msg->data, cv::IMREAD_COLOR);
    if (img.empty()) {
      RCLCPP_WARN(this->get_logger(), "Failed to decode annotated image");
      return;
    }
    
    cv::resize(img, img, cv::Size(), 0.5, 0.5);
    cv::imshow("Annotated Image", img);
    cv::waitKey(static_cast<int>(1000.0 / param::fps));
  }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ScreenNode>("screen_node");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}